/*
 *  DriverAgent.cpp
 *  powaah
 *
 *  Created by Tim Uusitalo on 4/23/11.
 *  Copyright 2011 __MyCompanyName__. All rights reserved.
 *
 */

#include "DriverAgent.h"

using namespace MathUtility;

DriverAgent::DriverAgent()
{
	options.reserve(100);
	
	// some precalculated acceleration values for each gear
	// calculated at forza track, but should be enough for our
	// look ahead-points
	// m/s^2
	acceleration[0] = 7.1666667f;
	acceleration[1] = 6.045f;
	acceleration[2] = 4.8783f;
	acceleration[3] = 4.05f;
	acceleration[4] = 2.81f;
	acceleration[5] = 2.04f;
	decceleration = -21.16f;
	
	// init our look ahead points
	for (float angle = -45.0f; angle <= 45.0f; angle += 5)
	{
		for (float ab = 0.0f; ab <= 1.0f; ab += 0.2f)
		{
			options.push_back(DriverOption(0.0f, convertDegToRad(angle)));
			options.back().setAccelerator(ab);
		}
		
		
		for (float ab = 0.6f; ab <= 1.0f; ab += 0.2f)
		{
			options.push_back(DriverOption(0.0f, convertDegToRad(angle)));
			options.back().setBrake(ab);
		}
	}
	counter = 0;	
}

DriverAgent::~DriverAgent()
{
	
}

void DriverAgent::update(CarState& cs, const float dt, TrackLearner& learner)
{

	counter++;

	int divide_by = 1;
	for (int i = 7; i < 12; i++)
	{
		if (cs.getTrack(i) >= 0)
		{
			target_speed += cs.getTrack(i);
			divide_by++;
			
		}
	}

	target_speed /= divide_by;

	speed_agent.setSpeed(target_speed*dt);

	
	
	// how far we should take us to the next point
	
	
	int gear = cs.getGear();
	
	if (gear > 1)
		gear--;
	else if (gear <1)
		gear = 0;
	
	if (counter % 1000 == 0)
	{
		std::cout << "#	" << cs.getSpeedX() << std::endl;
	}


	
	for (unsigned int i = 0; i < options.size(); i++)
	{
		float vel_current = cs.getSpeedX()/3.6f;
		float vel_res = vel_current;
        
		float accel = acceleration[gear] * options[i].getAccelerator();
		if (cs.getSpeedX() > 5.0f)
			accel += decceleration * options[i].getBrake(); 
		
        
		vel_res = vel_res + accel;
        
        
		options[i].setLength(((vel_current + vel_res)*0.5f)*dt);
		options[i].update(cs,dt);
		

		/*float angle = convertRadToDeg(options[i].getAngle());
		if (counter % 1000 == 0)
		{
			std::cout << options[i].getPosition().x << " " << options[i].getPosition().y << std::endl;
		}
		*/
	}
	
	if (counter % 1000 == 0)
	{
		counter = 0;
	}
	track_agent.update(cs,dt, learner);
	curve_agent.setLeftAndRight(track_agent.getDistToLeft(), track_agent.getDistToRight());
	curve_agent.update(cs,dt, learner, target_speed);
	//overtake_agent.update(cs,dt);

	speed_agent.update(cs,dt);
	
	track_agent.calculatePotentialForOptions(&options.front(), options.size());

	speed_agent.calculatePotentialForOptions(&options.front(), options.size());
	
	curve_agent.calculatePotentialForOptions(&options.front(), options.size());
	
	//overtake_agent.calculatePotentialForOptions(&options.front(), options.size());
	
	unsigned int pos = 0;
	
	
	for (unsigned int i = 1; i < options.size(); i++)
	{
		if (options[i].getPotential() > options[pos].getPotential())
		{
			pos = i;
		}
		//std::cout << options[i].getPotential() << std::endl;
	}
	
	//std::cout << "potential: " << options[pos].getPotential() <<  std::endl;
	highest = options[pos];
	
}

const DriverOption DriverAgent::getResultingOption()
{
	
	return highest;
}

const float DriverAgent::getSpeed(CarState& cs, const float rad)
{
	int pos = 0; 
	
	for ( int i = 0; i < 19; i++)
	{
		if (cs.getTrack(i) > cs.getTrack(pos))
		{
			pos = i;
		}
	}
	
	float target_speed = cs.getTrack(pos) * 3.6f;
	float radius;
	
	radius = pow(cs.getTrack(pos)*0.70f, 2);
	radius += (pow((track_agent.getDistToLeft() + track_agent.getDistToRight()), 2.0f));
	radius *= 0.5f;
	
	float speed = cs.getSpeedX() / 3.6f;
	speed *= speed;
	
	float Fa = (0.5f*(0.25f*12.f)*0.372f*1.3041f*speed) + (0.5f*(0.7f*18)*0.372f*1.3041f*speed);
	
	return sqrt((rad) * 1.035f * (9.82f+(Fa / 1100.f)));

}

const float DriverAgent::getWidth()
{
	return track_agent.getDistToLeft() + track_agent.getDistToRight();
}




const float DriverAgent::getTargetSpeed()
{
	return target_speed;
}
